function plan = plan_motion_display(S,T, M, orientation)

%plan_motion - Author Ryszard Szulc (assisted by Jack Young)
%T -  [NOW TARGET] array or 2 tuples
%M - Map array of tuples for each consecutive node
%orientation - angle from north in pi radians

% M=[0 0;20 0;20 15;40 15;40 20;0 20]
% % DOUBLE CHECK WHICH IS INITIAL AND WHICH IS TARGET BITS OF T!!!
% T=[5 5,15 17]

%Number of nodes in map and map co-ords
x_map = M(:,1)';
y_map = M(:,2)';
N= length(x_map) + 2; %N= nodes + (pos_now&target)

%CORRECT PROVIDING T STRUCTURE IS [NOW TARGET]
pos_now = S;
pos_target = T;

%new xy values including robot info.
x = [pos_now(1), pos_target(1), x_map];
y = [pos_now(2), pos_target(2), y_map];

%Create all possible combinations of vertices
% %NOTE: Need to zero-weight edges of the map itself, and inc padding.
%Similar to meshgrid for all possible, but omits same node matches
index = nchoosek(1:N,2);
%run linesegment map vs map+positions for intersects and disgard.
%Lines for map (references x_map y_map)
XYmap = [x_map;y_map]';
for j = 1:(length(XYmap)-1)
    XY1(j,:) = [XYmap(j,:),XYmap(j+1,:)];
end
XY1(j+1,:) =[XYmap(end,:),XYmap(1,:)];

%Lines for everything (referencing index, or x and y)
XYall = [x; y]';
for j=1:(length(index))
    XY2(j,:) = [XYall(index(j,1),:),XYall(index(j,2),:)];
end
%Takes map lines vs all lines and finds intersects that break map shape.
out = lineSegmentIntersect2(XY1,XY2);
%Adjacency matrix gives offending lines from robot/target position
A= out.intAdjacencyMatrix;
%Find offending points and remove from index of possible paths
[iInvalid,jInvalid] = find(A == 1);
index(jInvalid,:) = [];

index = index';
%Finds lines completely outside shape that line segment doesn't catch (ie.
%if right-angle made outside shape then will not intersect with another
%line, but stll not valid path.
for i = 1:length(index)
    center_points = [linspace(x(index(1,i))',x(index(2,i))',100);linspace(y(index(1,i))',y(index(2,i))',100)];
    invalidLines = find(~inpolygon(center_points(1,:),center_points(2,:),x_map',y_map'));
    if length(invalidLines) > 0
        index(:,i) = 0;
    end
end
[x_indices,y_indices] = find(index == 0);
index(:,y_indices) = [];
%Plot lines (possible paths) remaining in index matrix
figure;
line(x(index),y(index));

%Plot node points on top of line graph
hold on
plot(x,y,'rx')
plot(x(1),y(1),'ro')
plot(x(2),y(2),'ro')
axis equal

%Shortest path calculation start
adj_matrix = zeros(length(XYall),length(XYall));

line_lengths = sqrt((diff(x(index))).^2 + (diff(y(index))).^2);

for i = 1:length(index)
    adj_matrix(index(1,i),index(2,i)) = line_lengths(i);
end
adj_matrix = adj_matrix + adj_matrix';
adj_matrix(adj_matrix == 0) = Inf;

%Implements Djikstras algorithm on adjacency matrix.
[sp, spcost] = dijkstra(adj_matrix, 1,2);

target = XYall(sp(2),:);
point = XYall(sp(1),:);

dx = target(1) - point(1);
dy = target(2) - point(2);
theta = atan(dx/dy);
if dx < 0 & dy < 0
    theta = theta + abs(pi);
end
if dx < 0 & dy > 0
    theta = pi - abs(theta);
end
if dx > 0 & dy < 0
    theta = 2*pi-theta;
end
theta = theta  - orientation;
dist = sqrt(dx^2 + dy^2);
if (abs(theta) > 0.005)
    dist = 0;
end
if dist > 10
    dist = 10;
end
plan = [dist theta];